﻿
#if __linux__
#include <stdint.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/mman.h>
#include <pthread.h>
#include <sys/types.h>
#include <sys/prctl.h>
#include <sys/syscall.h>
#include <semaphore.h>
#include <errno.h>
#else
#include <Windows.h>
#endif

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <ctype.h>

#include "serial_usb.h"
#include "usb_udp.h"

#include "vdma.h"
#include "hardwares.h"
#include "printf.h"
#include "video_ps.h"

static vdma_handle VdmaHandle = { 0 };


// 时间戳
#define CLOCK_REALTIME	0
static inline uint64_t _get_now_us(void)
{
	struct timespec tm = { 0, };

	clock_gettime(CLOCK_REALTIME, &tm);

	return tm.tv_sec * (1000000llu) + tm.tv_nsec / 1000;
}
#define timeStat_var_get(var_)	uint64_t var_ = _get_now_us()



#if 1
// vdma操作
unsigned int vdma_get(vdma_handle *handle, int num)
{
	return handle->vdmaVirtualAddress[num >> 2];
}
void vdma_set(vdma_handle *handle, int num, unsigned int val)
{
	handle->vdmaVirtualAddress[num >> 2] = val;
}
void vdma_and(vdma_handle *handle, int num, unsigned int val)
{
	handle->vdmaVirtualAddress[num >> 2] &= val;
}
void vdma_or(vdma_handle *handle, int num, unsigned int val)
{
	handle->vdmaVirtualAddress[num >> 2] |= val;
}
void vdma_xor(vdma_handle *handle, int num, unsigned int val)
{
	handle->vdmaVirtualAddress[num >> 2] ^= val;
}

// buf标志操作
#define CAM_BUFFER_STATUS_A  	(0x12)
unsigned int vdma_get_flags(vdma_handle *handle)
{
	//return handle->bufStatusVAddress[0];
	return fpga_register_get(CAM_BUFFER_STATUS_A);
}
void vdma_set_flags(vdma_handle *handle, uint32_t val)
{
	//handle->bufStatusVAddress[0] = val;
	fpga_register_set(CAM_BUFFER_STATUS_A, val);
}
void vdma_and_flags(vdma_handle *handle, uint32_t val)
{
//	handle->bufStatusVAddress[0] &= val;
	fpga_register_and(CAM_BUFFER_STATUS_A, val);
}
void vdma_or_flags(vdma_handle *handle, uint32_t val)
{
//	handle->bufStatusVAddress[0] |= val;
	fpga_register_or(CAM_BUFFER_STATUS_A, val);
}
void vdma_xor_flags(vdma_handle *handle, uint32_t val)
{
//	handle->bufStatusVAddress[0] ^= val;
	fpga_register_xor(CAM_BUFFER_STATUS_A, val);
}

int vdma_idle(vdma_handle *handle)
{
	// Check whtether VDMA is transferring
	return (vdma_get(handle, OFFSET_VDMA_S2MM_STATUS_REGISTER) & VDMA_STATUS_REGISTER_FrameCountInterrupt) != 0;
}


int vdma_running(vdma_handle *handle)
{
	// Check whether VDMA is running, that is ready to start transfers
	return (vdma_get(handle, 0x34) & 1) == 1;
}


#define IO__MMAP_SIZE 0x1000
int vdma_setup(vdma_handle *handle,
               unsigned int baseAddr,
               int width, int height, int pixelLength,
               unsigned int fb1Addr,
               unsigned int fb2Addr,
               unsigned int fb3Addr)
{
#if __linux__
	// vdma flags状态GPIO的物理地址
	//const uint32_t	bufStautsPAddress = 0x41250000;

	handle->baseAddr = baseAddr;
	handle->width = width;
	handle->height = height;
	handle->pixelLength = pixelLength;
	handle->fbLength = pixelLength*width*height;
	handle->vdmaHandler = open("/dev/mem",  O_RDWR | O_SYNC);
	if (handle->vdmaHandler < 0)
	{
		dbg_error("open(/dev/mem) error\n");
		return -1;
	}
	printf("open(/dev/mem) ok\n");
	usleep(10000);
	

	handle->vdmaVirtualAddress = (unsigned int*)mmap(NULL, IO__MMAP_SIZE,
	                             PROT_READ | PROT_WRITE, MAP_SHARED,
	                             handle->vdmaHandler, (off_t)handle->baseAddr);
	if (handle->vdmaVirtualAddress == MAP_FAILED)
	{
		dbg_error("mmap(0x%08x) error\n", handle->baseAddr);
		return -1;
	}
	printf("mmap(0x%08x) ok:0x%08x\n", handle->baseAddr, (unsigned int)handle->vdmaVirtualAddress );
	usleep(10000);


	handle->fb1PhysicalAddress = (void*)fb1Addr;
	handle->fb1VirtualAddress = (unsigned char*)mmap(NULL, handle->fbLength,
	                            PROT_READ | PROT_WRITE, MAP_SHARED,
	                            handle->vdmaHandler, (off_t)fb1Addr);
	if (handle->fb1VirtualAddress == MAP_FAILED)
	{
		dbg_error("mmap(0x%08x) error\n", fb1Addr);
		return -2;
	}
	printf("mmap(0x%08x) ok:0x%08x\n", (uint32_t)handle->fb1PhysicalAddress, (uint32_t)handle->fb1VirtualAddress );
	usleep(10000);

	handle->fb2PhysicalAddress = (void*)fb2Addr;
	handle->fb2VirtualAddress = (unsigned char*)mmap(NULL, handle->fbLength,
	                            PROT_READ | PROT_WRITE, MAP_SHARED,
	                            handle->vdmaHandler, (off_t)fb2Addr);
	if (handle->fb2VirtualAddress == MAP_FAILED)
	{
		dbg_error("mmap(0x%08x) error\n", fb2Addr);
		return -3;
	}
	printf("mmap(0x%08x) ok:0x%08x\n", (uint32_t)handle->fb2PhysicalAddress, (uint32_t)handle->fb2VirtualAddress );
	usleep(10000);

	handle->fb3PhysicalAddress = (void*)fb3Addr;
	handle->fb3VirtualAddress = (unsigned char*)mmap(NULL, handle->fbLength,
	                            PROT_READ | PROT_WRITE, MAP_SHARED,
	                            handle->vdmaHandler, (off_t)fb3Addr);
	if (handle->fb3VirtualAddress == MAP_FAILED)
	{
		dbg_error("mmap(0x%08x) error\n", fb3Addr);
		return -3;
	}
	printf("mmap(0x%08x) ok:0x%08x\n", (uint32_t)handle->fb3PhysicalAddress, (uint32_t)handle->fb3VirtualAddress );
	usleep(10000);

	memset(handle->fb1VirtualAddress, 255, handle->width*handle->height*handle->pixelLength);
	memset(handle->fb2VirtualAddress, 255, handle->width*handle->height*handle->pixelLength);
	memset(handle->fb3VirtualAddress, 255, handle->width*handle->height*handle->pixelLength);

	handle->used = 1;
	
#endif

	printf("vdma_setup() ok\n");
	return 0;
}

void vdma_halt(void)
{
#if __linux__
	vdma_handle *handle = &VdmaHandle;

	if (handle->used  == 0)
		return;


	vdma_set(handle, OFFSET_VDMA_S2MM_CONTROL_REGISTER, VDMA_CONTROL_REGISTER_RESET);
	vdma_set(handle, OFFSET_VDMA_MM2S_CONTROL_REGISTER, VDMA_CONTROL_REGISTER_RESET);
	munmap((void *)handle->bufStatusVAddress, IO__MMAP_SIZE);
	munmap((void *)handle->vdmaVirtualAddress, IO__MMAP_SIZE);
	munmap((void *)handle->fb1VirtualAddress, handle->fbLength);
	munmap((void *)handle->fb2VirtualAddress, handle->fbLength);
	munmap((void *)handle->fb3VirtualAddress, handle->fbLength);
	close(handle->vdmaHandler);

	handle->used = 0;

	printf("vdma_halt() ok \n");
#endif
}

#endif



#if 1
void vdma_status_dump(int status)
{
	if (status & VDMA_STATUS_REGISTER_HALTED) printf(" halted");
	else printf("running");
	if (status & VDMA_STATUS_REGISTER_VDMAInternalError) printf(" vdma-internal-error");
	if (status & VDMA_STATUS_REGISTER_VDMASlaveError) printf(" vdma-slave-error");
	if (status & VDMA_STATUS_REGISTER_VDMADecodeError) printf(" vdma-decode-error");
	if (status & VDMA_STATUS_REGISTER_StartOfFrameEarlyError) printf(" start-of-frame-early-error");
	if (status & VDMA_STATUS_REGISTER_EndOfLineEarlyError) printf(" end-of-line-early-error");
	if (status & VDMA_STATUS_REGISTER_StartOfFrameLateError) printf(" start-of-frame-late-error");
	if (status & VDMA_STATUS_REGISTER_FrameCountInterrupt) printf(" frame-count-interrupt");
	if (status & VDMA_STATUS_REGISTER_DelayCountInterrupt) printf(" delay-count-interrupt");
	if (status & VDMA_STATUS_REGISTER_ErrorInterrupt) printf(" error-interrupt");
	if (status & VDMA_STATUS_REGISTER_EndOfLineLateError) printf(" end-of-line-late-error");
	printf(" frame-count:%d", (status & VDMA_STATUS_REGISTER_FrameCount) >> 16);
	printf(" delay-count:%d", (status & VDMA_STATUS_REGISTER_DelayCount) >> 24);
	printf("\n");
}

void vdma_s2mm_status_dump(vdma_handle *handle)
{
	int status = vdma_get(handle, OFFSET_VDMA_S2MM_STATUS_REGISTER);
	printf("S2MM status register (%08x):", status);
	vdma_status_dump(status);
}

void vdma_mm2s_status_dump(vdma_handle *handle)
{
	int status = vdma_get(handle, OFFSET_VDMA_MM2S_STATUS_REGISTER);
	printf("MM2S status register (%08x):", status);
	vdma_status_dump(status);
}

void vdma_start_triple_buffering(vdma_handle *handle)
{
	if (handle->used == 0)
		return;

	handle->flags = 0;
	vdma_set_flags(handle, handle->flags);

	// Reset VDMA
	vdma_set(handle, OFFSET_VDMA_S2MM_CONTROL_REGISTER, VDMA_CONTROL_REGISTER_RESET);
	//	vdma_set(handle, OFFSET_VDMA_MM2S_CONTROL_REGISTER, VDMA_CONTROL_REGISTER_RESET);
	usleep(1000);

	// Clear all error bits in status register
	vdma_set(handle, OFFSET_VDMA_S2MM_STATUS_REGISTER, -1);
	//	vdma_set(handle, OFFSET_VDMA_MM2S_STATUS_REGISTER, -1);

	// Do not mask interrupts
	vdma_set(handle, OFFSET_VDMA_S2MM_IRQ_MASK, 0xf);

	int interrupt_frame_count = 1;
	// Start both S2MM and MM2S in triple buffering mode
	vdma_set(handle, OFFSET_VDMA_S2MM_CONTROL_REGISTER,
	         (interrupt_frame_count << 16) |
	         VDMA_CONTROL_REGISTER_FrmCtn_IrqEn |
	         VDMA_CONTROL_REGISTER_GENLOCK_ENABLE |
	         VDMA_CONTROL_REGISTER_INTERNAL_GENLOCK |
	         VDMA_CONTROL_REGISTER_CIRCULAR_PARK |
	         VDMA_CONTROL_REGISTER_START);
	//	vdma_set(handle, OFFSET_VDMA_MM2S_CONTROL_REGISTER,
	//	         (interrupt_frame_count << 16) |
	//	         VDMA_CONTROL_REGISTER_START |
	//	         VDMA_CONTROL_REGISTER_GENLOCK_ENABLE |
	//	         VDMA_CONTROL_REGISTER_INTERNAL_GENLOCK |
	//	         VDMA_CONTROL_REGISTER_CIRCULAR_PARK);


	while ((vdma_get(handle, OFFSET_VDMA_S2MM_CONTROL_REGISTER) & 1) == 0 ||
	       (vdma_get(handle, OFFSET_VDMA_S2MM_STATUS_REGISTER ) & 1) == 1)
	{
		printf("%s,%d, Waiting for VDMA to start running...\n", __FILE__, __LINE__);
		usleep(1000);
	}

	// Extra register index, use first 16 frame pointer registers
	vdma_set(handle, OFFSET_VDMA_S2MM_REG_INDEX, 0);

	// Write physical addresses to control register
	vdma_set(handle, OFFSET_VDMA_S2MM_FRAMEBUFFER1, (unsigned int)handle->fb1PhysicalAddress);
	vdma_set(handle, OFFSET_VDMA_S2MM_FRAMEBUFFER2, (unsigned int)handle->fb2PhysicalAddress);
	vdma_set(handle, OFFSET_VDMA_S2MM_FRAMEBUFFER3, (unsigned int)handle->fb3PhysicalAddress);
	//vdma_set(handle, OFFSET_VDMA_MM2S_FRAMEBUFFER1, handle->fb2PhysicalAddress);
	//vdma_set(handle, OFFSET_VDMA_MM2S_FRAMEBUFFER2, handle->fb2PhysicalAddress);
	//vdma_set(handle, OFFSET_VDMA_MM2S_FRAMEBUFFER3, handle->fb2PhysicalAddress);

	// Write Park pointer register
	vdma_set(handle, OFFSET_PARK_PTR_REG, 0);

	// Frame delay and stride (bytes)
	vdma_set(handle, OFFSET_VDMA_S2MM_FRMDLY_STRIDE, handle->width*handle->pixelLength);
	//	vdma_set(handle, OFFSET_VDMA_MM2S_FRMDLY_STRIDE, handle->width*handle->pixelLength);

	// Write horizontal size (bytes)
	vdma_set(handle, OFFSET_VDMA_S2MM_HSIZE, handle->width*handle->pixelLength);
	//	vdma_set(handle, OFFSET_VDMA_MM2S_HSIZE, handle->width*handle->pixelLength);

	// Write vertical size (lines), this actually starts the transfer
	vdma_set(handle, OFFSET_VDMA_S2MM_VSIZE, handle->height);
	//	vdma_set(handle, OFFSET_VDMA_MM2S_VSIZE, handle->height);
}

#endif



#if 1
#define BUFFER_MAX_CNTS  	(3)
/********************************************************************//**
*\n 函数名称： uio_open()
*\n 功能描述：
*\n 		   打开/关闭uio中断接口。
*\n 输入参数：
*\n 		   char *dev_name
*\n 输出参数:
*\n
*\n 返 回 值:
*\n 		  0 成功， -1 失败
*\n
*\n -------------------------------------------------------------------
*\n  版本:	修改人:		 修改日期:		   描述:
*\n   V0.1	罗先能	 2018.3.16	  创建
************************************************************************/
static int uio_close(vdma_handle *handle)
{
	if (handle->uio_fd > 0)
	{
		close(handle->uio_fd);
		handle->uio_fd = 0;
	}

	return 0;
}

static int uio_open(vdma_handle *handle, char *dev_name)
{
	if (NULL == dev_name)
	{
		printf(" dev_name == NULL\n");
		return -1;
	}

	handle->index = -1;
	handle->index1 = -1;
	sem_init(&handle->sem, 0, 0);

	handle->uio_fd = open(dev_name, O_RDWR);
	if (handle->uio_fd <= 0)
	{
		perror(dev_name);
		dbg_error("Invalid UIO device file:%s.\n", dev_name);
		return -1;
	}
	printf("open(%s) == %d ok\n", dev_name, handle->uio_fd);
	return 0;
}

static int uio_wait(vdma_handle *handle, int wait_us)
{
	int ret = 0;
	if (handle->uio_fd <= 0)
	{
		dbg_error("uio_fd < 0 \n");
		usleep(10000);
		return -1;
	}

	int cnts = -1;
	cnts = read(handle->uio_fd, &cnts, 4);
	if (cnts != 4)
	{
		dbg_debug("uio read()==%d != 4, error\n", cnts);
		usleep(10000);
	}
	else
	{
		dbg_info("uio read(%d)  ok.\n", handle->uio_fd);
	}

	// 清中断
	int cr = vdma_get(handle, OFFSET_VDMA_S2MM_CONTROL_REGISTER);
	int sr = vdma_get(handle, OFFSET_VDMA_S2MM_STATUS_REGISTER);
	vdma_set(handle, OFFSET_VDMA_S2MM_STATUS_REGISTER, sr);

	int park = vdma_get(handle, OFFSET_PARK_PTR_REG);


	// 设置控制标志
	handle->index++;
	handle->flags |= 1 << (handle->index % BUFFER_MAX_CNTS);
	vdma_set_flags(handle, handle->flags);


	/* enable IRQ */
	static const int irq_on = 1;
	write(handle->uio_fd, &irq_on, sizeof(irq_on));

	// 通知处理线程
	sem_post(&handle->sem);

//	printf("irq(%d), cr(0x%08x),sr(0x%08x) \n",
//	      handle->index,  cr,        sr );

	return ret;
}

// 处理vdma中断
static void *task_vdma_interrupt(void *arg)
{
//	int policy;
//	struct sched_param param;
//	pthread_getschedparam(pthread_self(), &policy, &param);
//	PRINTF_DEBUG("SCHED_FIFO=%d, SCHED_RR=%d, SCHED_OTHER=%d \n", SCHED_FIFO, SCHED_RR, SCHED_OTHER);
//	PRINTF_DEBUG("thread policy:%d, priority:%d\n", policy, param.sched_priority);

	int ret = uio_open(&VdmaHandle, "/dev/uio0");
	if (ret < 0)
	{
		printf("uio_open(/dev/uio0) \n");
		goto error1;
	}
	printf("uio_open(uio0) ok\n");


	while(1)
	{
		uio_wait(&VdmaHandle, 100000);
	}


error1:
	pthread_exit(0);
}

int vdma_interrupt_init(void)
{
	// 设置线程优先级
	pthread_attr_t attr;
	pthread_attr_init(&attr);
	pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
	pthread_attr_setschedpolicy(&attr, SCHED_RR);
	struct sched_param param;
	param.sched_priority = 30;
	pthread_attr_setschedparam(&attr, &param);


	// 中断处理线程
	static pthread_t tid;
	int ret = pthread_create(&tid, &attr, task_vdma_interrupt, NULL);
	if(0 != ret)
	{
		perror("pthread_create() error \n");
		printf("pthread_create error : %s\n", strerror(errno));
		return (-1);
	}

	printf("vdma_interrupt_init() ok\n");
	return 0;
}

#endif



#if 1
/********************************************************************//**
*\n 函数名称： write_to_file()
*\n 功能描述：
*\n            buf数据写入到文件file
*\n 输入参数：
*\n            void* buf     ： buf数据
*\n            int32_t size  :  长度
*\n            char* file    :  文件名
*\n 输出参数:
*\n
*\n 返 回 值:
*\n           0 成功， -1 失败
*\n
*\n -------------------------------------------------------------------
*\n  版本:    修改人:       修改日期:         描述:
*\n   V0.1  罗先能    2018.5.11    创建
************************************************************************/
int32_t write_to_file(const char *file, void *pdata, int32_t size)
{
	FILE *pf = NULL;
	int ret = 0;
	pf = fopen(file, "wb+");

	if ( pf != NULL )
	{
		ret = fwrite(pdata, size, 1, pf);
	}

	if ( pf != NULL )
	{
		fclose(pf);
	}

	return ret;
}



// 读取task_vdma_interrupt中断处理线程接收到图像数据
void *vdma_task(void *arg)
{
	int ret;



	printf("vdma_task() ...\n");

	// 处理
	// AX7020: ov5640 @ VDMA1
	ret = vdma_setup(&VdmaHandle,
	                 0x41210000,
	                 CAM_HSIZE, CAM_VSIZE, CAM_PIXELLENGTH,
	                 0x0f000000, 0x0f200000, 0x0f400000);
	if (ret < 0)
	{
		printf("vdma_setup() error \n");
		//return -1;
		goto exit1;
	}

	vdma_start_triple_buffering(&VdmaHandle);
	printf("vdma_start_triple_buffering() ok\n");



	while(1)
	{
		//------------------------------------------------
		//  vdma 接收sensor数据
		//------------------------------------------------
		sem_wait(&(VdmaHandle.sem));  // 等待vdma中断
		printf("sem_wait() ok\n");
		
		VdmaHandle.index1++;
		int opt = VdmaHandle.index1 % BUFFER_MAX_CNTS;
		switch(opt)
		{
			default:
			{
				continue;
			}

			case 0: // 处理第1帧
			{
				//memcpy2hdmi(/*frame,*/ VdmaHandle.fb1VirtualAddress, CAM_HSIZE * CAM_VSIZE * CAM_PIXELLENGTH);
				memcpy2usb(/*frame,*/ VdmaHandle.fb1VirtualAddress, CAM_HSIZE * CAM_VSIZE * CAM_PIXELLENGTH);
				VdmaHandle.flags &=  ~1;
				vdma_set_flags(&VdmaHandle,  VdmaHandle.flags);

				VdmaHandle.value = VdmaHandle.value1;
				VdmaHandle.value1 = ((uint16_t)VdmaHandle.fb1VirtualAddress[1] << 8) + VdmaHandle.fb1VirtualAddress[0];
			}
			break;

			case 1: // 处理第2帧
			{
				//memcpy2hdmi(/*frame,*/ VdmaHandle.fb2VirtualAddress, CAM_HSIZE * CAM_VSIZE * CAM_PIXELLENGTH);
				memcpy2usb(/*frame,*/ VdmaHandle.fb2VirtualAddress, CAM_HSIZE * CAM_VSIZE * CAM_PIXELLENGTH);
				VdmaHandle.flags &=  ~2;
				vdma_set_flags(&VdmaHandle,  VdmaHandle.flags);

				VdmaHandle.value = VdmaHandle.value1;
				VdmaHandle.value1 = ((uint16_t)VdmaHandle.fb2VirtualAddress[1] << 8) + VdmaHandle.fb2VirtualAddress[0];
			}
			break;

			case 2: // 处理第3帧
			{
				//memcpy2hdmi(/*frame,*/ VdmaHandle.fb3VirtualAddress, CAM_HSIZE * CAM_VSIZE * CAM_PIXELLENGTH);
				memcpy2usb(/*frame,*/ VdmaHandle.fb3VirtualAddress, CAM_HSIZE * CAM_VSIZE * CAM_PIXELLENGTH);
				VdmaHandle.flags &=  ~4;
				vdma_set_flags(&VdmaHandle,  VdmaHandle.flags);

				VdmaHandle.value = VdmaHandle.value1;
				VdmaHandle.value1 = ((uint16_t)VdmaHandle.fb3VirtualAddress[1] << 8) + VdmaHandle.fb3VirtualAddress[0];

				// 随机延时，产生丢帧错误
				//int us = (rand() % 100) * 1000;
				//int us = 500 * 1000;
				//usleep(us);
			}
			break;
		}


		usb_show_yuv422();  // usb显示数据
#if 1
		// 图像数据存储到文件
		if (VdmaHandle.index1 == 99)
		{
			static int save_flag = 0;
			if (save_flag == 0)
			{
				save_flag = 0xaa;

				write_to_file("/camera.yuv", VdmaHandle.fb1VirtualAddress, CAM_HSIZE * CAM_VSIZE * 2);
				printf( "write /camera.yuv \n");

				//write_to_file("/mnt/sd/hdmi_in.bin",  frame,  CAM_HSIZE * CAM_VSIZE * 2);
				//printf( "write /mnt/sd/hdmi_in.bin \n");
				
				//THdmi *p = phdmi;
				//write_to_file("/mnt/sd/hdmi_out.bin", p->pbuffer, p->width * p->height * 3);
				//printf( "write /mnt/sd/hdmi_out.bin\n");
			}
		}
#endif

	}  //while(1)

exit1:
	pthread_exit(NULL);

}

int  vdma_init(void)
{
	int ret = 0;

	fpga_register_open();
	printf("fpga_register_open() ok\n");
	usleep(100*1000);

	// vdma 中断处理
	ret = vdma_interrupt_init();
	if (ret < 0)
	{
		printf("vdma_interrupt_init() error\n");
		return -1;
	}
	usleep(100*1000);
	printf("sleep ok\n");
	
	static pthread_t vdma_tid = 0;
	ret = pthread_create(&vdma_tid, NULL, vdma_task, NULL);
	if(0 != ret)
	{
		printf("pthread_create failed : %s\n", strerror(errno));
		return (-1);
	}
	usleep(100*1000);
	printf("sleep ok\n");

	printf("vdma_init() ok\n");
	return 0;
}
#endif



